#include "encoder_task.h"
#include "FreeRTOS.h"
#include "task.h"
#include "ti_msp_dl_config.h"  // 根据实际硬件驱动调整




#include "encoder.h"
#include <stdio.h>
#include "pid.h"




#define user_encoder_DEBUG

#ifdef user_encoder_DEBUG


#define user_encoder_printf(format, ...)printf( format "\r\n", ##__VA_ARGS__)
#define user_encoder_info(format, ...)printf("info:" format "\r\n", ##__VA_ARGS__)
#define user_encoder_debug(format, ...)printf("encoder_debug:"  format "\r\n", ##__VA_ARGS__)
#define user_encoder_error(format, ...)printf("error:"  format "\r\n", ##__VA_ARGS__)
#else
#define user_encoder_printf(format, ...)
#define user_encoder_info(format, ...)
#define user_encoder_debug(format, ...)
#define user_encoder_error(format, ...)
#endif
/* 私有函数和变量（仅本文件可见） */
// static TaskHandle_t xMotorTaskHandle = NULL;  // 任务句柄
static int flag = 0;




/* 任务主函数 */
void EncoderTask(void *pvParameters) 
{
    // user_encoder_debug("succed!!");
    uint16_t last_count = 0,new_count = 0;
    int16_t  count_v = 0;


    DL_TimerG_setCounterMode(QEI_0_INST, DL_TIMER_COUNT_MODE_UP);

     DL_TimerG_enableInterrupt(QEI_0_INST, DL_TIMER_INTERRUPT_DC_EVENT);
    NVIC_EnableIRQ(QEI_0_INST_INT_IRQN);

    DL_Timer_startCounter(QEI_0_INST);
    
    uint16_t new_time = 0,last_time = 0,all_time = 0;

    /* 任务主循环 */
    for (;;) {
    new_count = DL_Timer_getTimerCount(QEI_0_INST);
    // new_time  = DL_Timer_getTimerCount(TIMER_0_INST);
    // user_encoder_debug("count = %d",new_count);
    
    count_v = (int16_t)new_count - (int16_t)last_count;
    // user_encoder_debug("V = %d",count_v);
    // user_encoder_debug("%d",new_time);
    // if(new_time > last_time)
    // {
	//     pid_speed.actual_val = count_v / 4096.0 *360 *2 /((new_time - last_time)/10000.0)*1.0;   
    // }
    // else {
        // pid_speed.actual_val = count_v / 4096.0 *360 *2/((20000-last_time +new_time) / 10000.0) *1.0;
    // }
pid_speed.actual_val = count_v / 4096.0 *360 *2/(20000 / 10000.0) *1.0;
    if(pid_location_flag == 1)
    {
        pid_location.actual_val += count_v /4096.0 * 360.0 *2;  //磁环的累计角度
    }

    
    // user_encoder_debug("angel = %f",pid_location.actual_val);

    // user_encoder_debug("V=%lf",pid_speed.actual_val);


    last_count = new_count;
		    
        /* 延迟10ms（根据需求调整） */
        vTaskDelay(pdMS_TO_TICKS(1));
    }
    
    /* 理论上不会执行到这里 */
    vTaskDelete(NULL);
}

void QEI_0_INST_IRQHandler(void)
{
    switch (DL_TimerG_getPendingInterrupt(QEI_0_INST)) {
        case DL_TIMER_IIDX_DIR_CHANGE:
            if (DL_TimerG_getQEIDirection(QEI_0_INST) == DL_TIMER_QEI_DIR_DOWN)
                {DL_TimerG_setCounterMode(QEI_0_INST, DL_TIMER_COUNT_MODE_UP);
                DL_TimerG_clearInterruptStatus(QEI_0_INST, DL_TIMER_INTERRUPT_DC_EVENT);}
            else
                {DL_TimerG_setCounterMode(QEI_0_INST, DL_TIMER_COUNT_MODE_DOWN);
                DL_TimerG_clearInterruptStatus(QEI_0_INST, DL_TIMER_INTERRUPT_DC_EVENT);}
        default:
            break;
    }
}